/**
*@Created by YChienHung on 20-09-25.
*/

#include <iostream>
#include <cmath>
#include "opencv2/opencv.hpp"

#define WINDWOS_NAME "result2"

using namespace std;
using namespace cv;

typedef struct {
    int x;
    int y;
    int r;
} Center;//定义巨轮结构体

vector<Point> s_CenterPos; //各个巨轮子叶中心坐标
Center s_CenterPoint;      //巨轮中心坐标

int s_Speed;                                   //风扇的速度
Point s_ActivationRectPos(0, 0);       //激活子叶当前的位置
Point s_LastActivationRectPos(0, 0);   //激活子叶上一次的位置

/**
 * 利用三点获取中心坐标的位置，并将其保存到全局变量中
 * @param x1
 * @param y1
 * @param x2
 * @param y2
 * @param x3
 * @param y3
 */
void getCenterPos(int x1, int y1, int x2, int y2, int x3, int y3) {
    double a = 2 * (x2 - x1);
    double b = 2 * (y2 - y1);
    double c = x2 * x2 + y2 * y2 - x1 * x1 - y1 * y1;
    double d = 2 * (x3 - x2);
    double e = 2 * (y3 - y2);
    double f = x3 * x3 + y3 * y3 - x2 * x2 - y2 * y2;
    double x = (b * f - e * c) / (b * d - e * a);
    double y = (d * c - a * f) / (b * d - e * a);
    s_CenterPoint.x = x;
    s_CenterPoint.y = y;
    s_CenterPoint.r = sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1));//半径
}

/**
 * get angle ACB, point C is the center point,and 弧度值
 * @param x1
 * @param y1
 * @param x2
 * @param y2
 * @param x3
 * @param y3
 * @return
 */
double get_angle(double x1, double y1, double x2, double y2, double x3, double y3) {
    double theta = atan2(x1 - x3, y1 - y3) - atan2(x2 - x3, y2 - y3);
    if (theta > M_PI)
        theta -= 2 * M_PI;
    if (theta < -M_PI)
        theta += 2 * M_PI;

    theta = abs(theta);
    return theta;
}

int main(int argc, char *args[]) {
    Mat img,img_red;
    Mat frame;            //视频的数据流
    VideoCapture cap = VideoCapture("2.mp4");

    double angle;                   //角度, 弧度值
    bool bIsFoundActivation = false;//是否找到了激活的子叶
    bool bIsFoundCenter = false;    //是否找到了中心坐标

    String str_speed;               //显示速度的文本
    ostringstream ostr;             //格式化数据流

    if (cap.isOpened()) {
        for (;;) {
            cap >> frame;
            if (frame.empty())
                break;

            //B G R
            vector<Mat> channels;
            split(frame, channels);  //分离色图像色彩通道，则三通道图像成为三个单通道图像
            //img_b = channels.at(0);
            //img_g = channels.at(1);
            img_red = channels.at(2);

            //二值化处理
            threshold(img_red, img_red, 190, 255, THRESH_BINARY);   //根据直方图来计算阈值
            //threshold(img_g, img_g, 100, 255, THRESH_BINARY);//根据直方图来计算阈值

            threshold(frame, img, 170, 255, THRESH_BINARY);//根据直方图来计算阈值
            cvtColor(img, img, CV_BGR2GRAY);                              //二值化图像

            //处理杂点
            Mat kernel = getStructuringElement(MORPH_RECT, Size(6, 6));
            erode(img_red, img_red, kernel);  //腐蚀
            dilate(img_red, img_red, kernel); //膨胀

            //erode(img_g, img_g, kernel);  //腐蚀
            //dilate(img_g, img_g, kernel); //膨胀

            erode(img, img, kernel);  //腐蚀
            dilate(img, img, kernel); //膨胀

            //寻找轮廓
            vector<vector<Point>> contours;
            vector<Vec4i> hierarchy;

            findContours(img, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point());
            for (int i = 0; i < contours.size(); i++) {
                //contours[i]代表的是第i个轮廓, contours[i].size()代表的是第i个轮廓上所有的像素点数
                Point leftup(99999, 99999), rightdown(0, 0);
                for (int j = 0; j < contours[i].size(); j++) {
                    Point P = Point(contours[i][j].x, contours[i][j].y);

                    if (P.x < leftup.x)
                        leftup = P;
                    if (P.x > rightdown.x)
                        rightdown = P;
                }

                if (s_CenterPos.size() < 3) {
                    s_CenterPos.push_back(Point(leftup.x / 2 + rightdown.x / 2, leftup.y / 2 + rightdown.y / 2));
                }

                //读取原图像(y, x)的BGR颜色值，如果是灰度图像, 将Vec3b改为uchar
                //这里面有一个坑, 详情见笔记
                Scalar color = frame.at<Vec3b>(Point(leftup.x / 2 + rightdown.x / 2, \
                                                         leftup.y / 2 + rightdown.y /2));
                //B G R
                if (color(1) < 100 && color(2) > 200) {

                    //记录激活矩形中心坐标
                    s_LastActivationRectPos = s_ActivationRectPos;//用于计算速度

                    s_ActivationRectPos.x = leftup.x / 2 + rightdown.x / 2;
                    s_ActivationRectPos.y = leftup.y / 2 + rightdown.y / 2;

                    bIsFoundActivation = true;
                }
            }

            //防止出现特征点不够的情况
            if (s_CenterPos.size() == 3) {
                bIsFoundCenter = true;
                getCenterPos(s_CenterPos[0].x, s_CenterPos[0].y, s_CenterPos[1].x, s_CenterPos[1].y, s_CenterPos[2].x, s_CenterPos[2].y);
                s_CenterPos.clear();//清空, 准备下一次数据

                if (s_LastActivationRectPos.x != 0) {
                    angle = get_angle(s_LastActivationRectPos.x, s_LastActivationRectPos.y, s_ActivationRectPos.x,
                                      s_ActivationRectPos.y, s_CenterPoint.x, s_CenterPoint.y);
                    s_Speed = angle * s_CenterPoint.r / 30 * 1000;// 计算速度
                }
            }

            //标记关键信息
            if (bIsFoundCenter && bIsFoundActivation) {
                //标记中心位置
                //circle(frame, Point(s_CenterPoint.x, s_CenterPoint.y), 10, Scalar(255, 0, 0), 2);

                //去除开始那段没速度的问题
                if (s_Speed) {

                    ostr.str("");//清空数据流, 重复利用
                    ostr << "speed: " << s_Speed << " Pixels Per Second";
                    str_speed = ostr.str();

                    putText(frame, str_speed, Point(0, 50), FONT_HERSHEY_COMPLEX, 2, Scalar(0, 0, 255), 2);
                }
            }
            if (bIsFoundActivation)//标记红色的大伞叶
                rectangle(frame, Point(s_ActivationRectPos.x - 20, s_ActivationRectPos.y - 20),
                          Point(s_ActivationRectPos.x + 20, s_ActivationRectPos.y + 20), Scalar(125, 125, 125), 2);

            //清除获取标记
            bIsFoundActivation = false;
            bIsFoundCenter = true;

            //显示图像
            imshow(WINDWOS_NAME, frame);

            uint8_t key = waitKey(30);

            if (key == 'q' || key == 'Q')
                break;
        }
    }

    cap.release();// 释放视频句柄
    destroyAllWindows();//关闭所有窗口
    return 0;
}